/***************************************************************************
创建者：彭秋明
 开始时间：         : 2017.03.01
 copyright            : (C) 同川科技
 修改说明：（每次有修改就添加一条，带有 修改人，修改时间，修改描述）

 ***************************************************************************
 *   定义各种求解器，包括Pos、Vel、Acc的正逆解
 *
*                                                                         *
 ***************************************************************************/

#ifndef SOLVERTEST_HPP
#define SOLVERTEST_HPP

#include "models.hpp"
#include "measuretime.h"
#include <kdl/chain.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainfksolvervel_recursive.hpp>
#include <kdl/chainidsolver_recursive_newton_euler.hpp>
#include <kdl/chainidsolver_vereshchagin.hpp>
#include <kdl/chainiksolverpos_lma.hpp>
#include <kdl/chainiksolverpos_nr.hpp>
#include <kdl/chainiksolverpos_nr_jl.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/chainiksolvervel_pinv_givens.hpp>
#include <kdl/chainiksolvervel_pinv_nso.hpp>
#include <kdl/chainiksolvervel_wdls.hpp>
#include <fstream>

using namespace KDL;

class SolverTest
{
public:
    SolverTest();
    //输出文件，记录求解程序测试时间各项数据
    std::ofstream outfile;
    std::string str;

    void writeSolverTestTime(std::string str, MeasureTime* timeTest);
    void chainFkSolverPosTest();
    void chainIkSolverPosTest_LMA();
    void chainIkSolverPosTest_NR();
    void chainIkSolverPosTest_NR_JL();
    void chainFkVelAndIkVelTest();
    void chainIkVelTest_NSO();
    void chainFkAccAndIkAccTest();
    void chainIdSolverTest_RNE();
    void chainIdSolverTest_Vereshchagin();
    ~SolverTest();

private:
    Chain puma;
    int jnPuma;
    JntArray jointPuma;

    void chainIkSolverPos(const JntArray& joint, ChainFkSolverPos& fkSolverPos, JntArray& jointInit,
                          ChainIkSolverPos& ikSolverPos, JntArray& jointOut, std::string str);
    void chainSolverVel(const Chain& chain,  ChainFkSolverVel& fkSolverVel, ChainIkSolverVel& ikSolverVel, std::string str);
    //void chainSolverAcc(const Chain& chain, ChainFkSolverAcc& fkSolverAcc, ChainIkSolverAcc& ikSolverAcc);
};


#endif // SOLVERTEST_HPP
